#include <mc_path_planner/graph_planner/node_2d.h>

// 8邻域
const int Node2D::dir = 8;
const int Node2D::dx[] = { -1, -1, 0, 1, 1, 1, 0, -1 };
const int Node2D::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };

// 比较节点是否相同
bool Node2D::operator == (const Node2D& rhs) const {
  return x == rhs.x && y == rhs.y;
}

// 启发函数
float Node2D::movementCost(const Node2D& pred) const { 
    // // 计算欧几里得距离
    // float dx = this->x - pred.getX();
    // float dy = this->y - pred.getY();
    // return hypot(dx, dy);

    // 计算曼哈顿距离
    float dx = this->x - pred.getX();
    float dy = this->y - pred.getY();
    return abs(dx) + abs(dy);

    // // 计算切比雪夫距离
    // float dx = std::abs(this->x - pred.getX());
    // float dy = std::abs(this->y - pred.getY());
    // return std::max(dx, dy);
}
